/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.DigitalInput;
//import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.ArmStop;

/**
 *
 * @author Team 3502
 */
public class BridgeArm extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    //private Relay arm;
    private SpeedController arm;
    private DigitalInput armswitch;

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new ArmStop());
    }
    
    public BridgeArm(){
        //arm = new Relay(RobotMap.armMotor);
        arm = new Jaguar(RobotMap.armMotor);
        armswitch = new DigitalInput(RobotMap.armSwitch);
    }

    public void stow(){
        //arm.set(Relay.Value.kReverse);
        arm.set(-1.0);
    }

    public void extend(){
        if (armswitch.get() == true){
            //arm.set(Relay.Value.kForward);
            System.out.println("armswitch.get = " + armswitch.get());
            arm.set(1.0);
        }
        else {
            //arm.set(Relay.Value.kOff);
            arm.set(0.0);
        }        
    }

    public void stop(){
        //arm.set(Relay.Value.kOff);
        arm.set(0.0);
    }
}